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ABSTRACT 

This  report  presents  an  algorithm  for  efficiently  solving  the  Simultaneous  Localisation 
and  Map  Building  (SLAM)  problem.  The  SLAM  problem  requires  both  the  dynamic 
estimation  of  the  sensor  location  and  the  tracking  of  features  of  interest  in  the  environ¬ 
ment  using  the  sensor  measurements.  The  problem  is  difficult  because  the  unknown 
sensor  and  feature  locations  are  coupled  through  the  sensor  measurement.  It  has  been 
shown  that  under  linear  Gaussian  conditions,  a  Kalman  Filter  solution  converges  to  a 
solution  relative  to  the  unknown  starting  location.  However,  this  approach  does  not 
scale  well  with  the  number  of  features  in  the  scene,  and  is  unfeasible  for  large  maps. 
The  algorithm  introduced  here  is  based  on  the  Probabilistic  Multi-Hypothesis  Tracker 
(PMHT)  and  exploits  a  factorisation  of  the  problem  to  reduce  the  computational  re¬ 
quirements  of  the  Kalman  Filter  approach.  The  new  algorithm  is  demonstrated  on  a 
benchmark  data  set  recorded  in  Victoria  Park. 
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Simultaneous  Localisation  and  Map  Building  using  the  Probabilistic 

Multi-Hypothesis  Tracker 


EXECUTIVE  SUMMARY 

This  report  introduces  a  new  algorithm  for  solving  the  feature-based  Simultaneous  Localisa¬ 
tion  and  Map  Building  problem  (SLAM).  In  feature-based  SLAM,  one  or  more  platforms  move 
through  an  unknown  environment  containing  stationary  objects  (landmarks)  that  can  be  observed 
with  onboard  sensors.  The  aim  is  to  simultaneously  estimate  the  position  and  orientation  of  the 
platform,  while  producing  a  map  of  the  landmarks.  SLAM  provides  a  method  for  frame-to-frame 
registration  of  sensor  observations  when  navigation  information  is  lacking,  or  not  of  high  enough 
accuracy. 

The  standard  approach  to  feature-based  SLAM  is  to  stack  all  of  the  unknowns  into  a  system 
state  vector,  and  then  use  a  Kalman  Filter  to  estimate  the  state  values.  However,  this  state  vector 
becomes  very  large  as  the  number  of  landmarks  increases,  and  the  algorithm  resource  requirements 
scale  as  the  square  of  the  number  of  unknowns.  This  makes  the  direct  Kalman  Filter  approach 
infeasible  for  large  maps.  In  addition,  the  Kalman  Filter  approach  assumes  that  the  association 
of  measurements  to  landmarks  is  known,  and  is  only  compatible  with  simple  data  association 
methods. 

The  algorithm  presented  here  is  based  on  the  Probabilistic  Multi-Hypothesis  Tracker  (PMHT), 
which  is  a  multi-target  tracking  approach  that  provides  good  scaling  properties  with  the  problem 
size.  By  generalising  the  standard  PMHT,  an  algorithm  is  found  that  scales  linearly  with  the 
number  of  landmarks  in  the  map,  while  offering  performance  at  least  as  good  as  the  Kalman  Filter 
approach. 

The  performance  of  the  PMHT  SLAM  algorithm  is  analysed  using  simulated  data  and  com¬ 
pared  with  that  of  the  Kalman  Filter  approach.  PMHT  incorporates  a  kind  of  probabilistic  data 
association,  and  this  is  demonstrated  to  give  significant  performance  improvements  when  mea¬ 
surement  errors  are  high.  The  performance  of  the  algorithm  is  also  presented  for  a  benchmark 
SLAM  data  set  recorded  by  the  University  of  Sydney.  The  algorithm  output  is  shown  to  be  com¬ 
parable  with  that  achieved  by  state-of-the-art  algorithms,  even  when  navigation  measurements  are 
denied. 
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Notation 


assignment  index  for  the  rth  measurement  from  sensor  n  at  scan  t 

(the  true  measurement  source) 

all  assignments  for  sensor  n  at  scan  t 

all  assignments  at  scan  t 

all  assignments 

number  of  landmarks  (targets)  in  world 

number  of  landmarks  in  field-of-view 

number  of  sensors 

EM  auxiliary  function 

number  of  scans  (batch  length) 

state  of  landmark  m  at  scan  t 

all  landmark  of  target  m 

all  landmark  states 

state  of  sensor  n  at  scan  t 

all  states  of  sensor  n 

all  sensor  states 

the  rth  measurement  at  scan  t  from  sensor  n 
all  measurements  at  scan  t  from  sensor  n 
all  measurements  at  scan  t 
all  measurements 
measurement  pdf 

number  of  observations  at  from  sensor  n  at  scan  t 

assignment  prior,  P(ktnr  =  m) 

set  of  all  assignment  priors 

the  state  prior  for  sensor  n 

the  state  evolution  pdf  for  sensor  n 

the  state  prior  for  target  m 

the  state  evolution  pdf  for  target  m 

estimate  calculated  on  the  ith  EM  iteration 
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1  Introduction 

The  standard  target  tracking  problem  is  to  estimate  the  location  (and  perhaps  other  parameters) 
of  scatterers  in  the  sensor  field  of  view  using  sensor  measurements.  It  is  usually  assumed  that  the 
sensor  location  and  orientation  are  known  with  sufficient  accuracy  [Blackman  &  Popoli  1999]. 
A  notable  exception  is  the  registration  problem,  when  multiple  sensors  are  fused  together.  In 
registration,  the  sensor  position  and  orientation  are  known  to  within  a  small  error,  and  this  error 
is  estimated  by  comparing  measurements  or  tracks  on  common  targets  (e.g.  [Bar-Shalom  &  Blair 
2000]).  Navigation  is  the  reverse  of  the  tracking  problem.  Here  the  locations  of  the  features  are 
known  (the  sensor  has  a  map)  and  the  dynamic  sensor  position  is  estimated  (e.g.  [Bar-Shalom 
et  al.  2001]). 

The  problem  where  both  the  sensor  position  and  the  feature  (target)  locations  are  to  be  esti¬ 
mated  is  referred  to  as  Simultaneous  Localisation  and  Map  Building  (SLAM)  or  Concurrent  Map 
Building  and  Localisation  (CML).  The  inherent  difficulty  in  the  SLAM  problem  is  that  the  sensor 
data  is  generally  a  nonlinear  function  of  the  relative  difference  between  the  sensor  and  feature 
positions.  This  coupling  of  the  unknowns  through  the  observation  process  necessitates  a  joint 
estimation  scheme. 

Numerous  methods  have  been  used  to  address  the  SLAM  problem.  [Thrun  2002]  gives  a 
good  overview  of  mapping  and  SLAM  approaches.  This  report  will  be  concerned  with  feature- 
based  SLAM  where  the  environmental  map  is  composed  of  a  set  of  discrete  stationary  objects 
(landmarks).  The  benchmark  method  for  solving  this  problem  is  to  use  a  Kalman  Filter  with  a 
state  vector  consisting  of  the  platform  location  and  all  of  the  landmark  locations  stacked  together. 
When  the  system  is  linear  and  the  random  elements  are  Gaussian,  this  is  an  optimal  approach, 
and  it  has  been  shown  that  the  map  derived  converges  to  a  relative  map  with  zero  uncertainty 
[  Dissanayake  et  al.  2001  ].  However,  the  approach  does  not  scale  well  with  large  maps.  If  there  are 
M  landmarks,  then  the  length  of  the  state  vector  is  proportional  to  M  and  the  covariance  matrix  is 
proportional  to  M2  (ignoring  the  contribution  of  the  sensor  state).  This  means  that  the  algorithm's 
memory  requirement  grows  as  M2  and  its  computation  requirement  grows  at  least  as  M2  1 .  For 
this  reason,  the  stacked  state  vector  approach  is  infeasible  for  large  maps  (large  M). 

One  solution  to  the  complexity  of  the  stacked  state  vector  approach  is  to  update  only  those 
landmarks  within  the  locale  of  the  platform  for  most  measurements.  The  information  about  other 
landmarks  garnered  from  these  measurements  comes  only  through  improved  knowledge  of  the 
platform,  and  can  be  incorporated  using  global  updates  at  a  slower  rate.  An  algorithm  based  on  this 
principle  is  presented  in  [Guivant  &  Nebot  2001a].  This  method  still  has  quadratic  complexity  in 
the  number  of  landmarks  inside  the  field  of  view,  but  it  is  somewhat  insensitive  to  the  total  number 
of  landmarks  in  the  world.  In  contrast,  the  standard  stacked  state  vector  has  quadratic  complexity 
in  the  total  number  of  landmarks.  If  the  number  of  landmarks  currently  in  view  is  much  smaller 
than  the  total  number  (which  is  the  case  for  many  problems)  then  this  represents  a  considerable 
saving. 

The  stacked  state  vector  methods  explicitly  assume  that  the  association  of  measurements  to 
landmarks  is  known.  In  practice,  this  means  that  nearest  neighbour  association  would  be  used. 
Other  data  association  approaches,  such  as  probabilistic  data  association,  or  Multi-Hypothesis 
Tracking  could  be  adapted,  but  given  that  the  problem  generally  has  a  large  number  of  landmarks, 

Multiplications  involving  the  covariance  matrix  will  be  order  M3  unless  the  special  structure  of  the  matrices  in¬ 
volved  is  exploited. 


1 


DSTO-TR-1691 


and  the  computation  cost  with  even  nearest  neighbours  is  prohibitive,  it  is  unlikely  that  these 
approaches  could  be  feasibly  implemented. 

An  alternative  approach  to  stacking  the  state  vector  is  to  exploit  the  Bayesian  factorisation 
of  the  problem.  Conditioned  on  the  sensor  location,  the  landmark  estimation  problem  is  in¬ 
dependent  for  the  different  landmarks.  This  allows  parallel  filtering,  which  is  hence  linear  in 
the  number  of  landmarks.  The  difficulty  then  becomes  tracking  the  sensor  position.  One  algo¬ 
rithm  based  on  this  approach  is  referred  to  as  FastSLAM  [Montemerlo  et  al.  2002,  Montemerlo 
et  al.  2003,  Montemerlo  2003].  FastSLAM  uses  a  particle  filter  to  track  the  sensor  location,  and 
parallel  Extended  Kalman  Filters  to  track  the  landmarks.  The  particle  filter  is  a  Monte  Carlo  based 
estimation  technique  [Doucet  et  al.  2001].  The  sensor  pdf  is  represented  by  a  collection  of  Mv 
random  samples,  referred  to  as  particles.  For  FastSLAM,  each  particle  performs  parallel  EKFs 
for  each  landmark.  Since  the  conditional  pdf  of  the  landmarks  is  tracked,  no  information  about  a 
landmark  is  gained  unless  it  is  observed.  FastSLAM  uses  this  fact  to  reduce  the  work  it  needs  to 
do  for  the  unobserved  landmarks.  FastSLAM  claims  a  complexity  of  log2  M  and  MpMOL ,  where 
Mpov  is  the  number  of  landmarks  currently  in  the  sensor  filed  of  view.  If  Mpov  M  and  N 
is  not  too  large,  then  FastSLAM  is  more  efficient  than  the  Kalman  Filter  approach.  Although  the 
computation  requirements  scale  well,  they  remain  quite  high  [Guivant  &  Nebot  200 1ft]. 

Besides  the  computational  aspects  of  FastSLAM,  there  are  other  advantages  of  the  approach. 
The  particle  filter  it  uses  is  not  based  on  linear  Gaussian  assumptions,  and  is  a  preferred  method 
for  nonlinear  non-Gaussian  filtering  (e.g.  [Ristic  et  al.  2004]).  In  applications  where  the  sensor 
passes  close  to  landmarks,  such  as  with  ground  vehicles,  the  nonlinearity  may  be  high,  and  the 
Extended  Kalman  Filter  may  not  be  adequate  for  the  problem.  FastSLAM  also  has  an  in-built  data 
association  ability  by  using  the  particles  to  sample  the  different  possible  assignment  combinations. 
Thus  the  sensor  filter  uses  a  Monte  Carlo  approximation  for  the  data  association  problem  as  well 
as  the  nonlinear  filtering. 

This  report  will  present  a  different  algorithm,  similarly  based  on  the  Bayesian  factorisation 
of  the  SLAM  problem.  This  algorithm  uses  Probabilistic  Multi-Hypothesis  Tracking  to  achieve 
a  complexity  only  linear  in  the  number  of  targets,  and  uses  probabilistic  data  association.  The 
Probabilistic  Multi-Hypothesis  Tracker  (PMHT)  of  [Streit  &  Luginbuhl  1995]  is  an  algorithm 
derived  from  the  application  of  the  Expectation  Maximisation  (EM)  of  [Dempster  et  al.  1977]  to 
the  tracking  problem.  The  standard  PMHT  models  the  assignment  of  measurements  to  targets 
as  hidden  variables  (nuisance  parameters)  and  estimates  target  states  by  taking  the  expectation 
over  the  assignments.  Unlike  other  tracking  approaches,  PMHT  assumes  that  the  assignment  of 
each  measurement  is  an  independent  realisation  of  a  random  process.  This  assumption  allows 
the  PMHT  to  be  optimally  realised  with  linear  complexity  in  the  number  of  targets  (i.e.  Mpov )• 
PMHT  is  a  data  association  approach,  and  the  estimation  scheme  used  depends  on  the  problem. 
For  linear  Gaussian  statistics,  the  PMHT  can  be  implemented  as  a  bank  of  parallel  Kalman  Filters. 
For  nonlinear  problems,  particle  filters  have  been  used. 

This  report  demonstrates  how  a  generalisation  of  the  PMHT  can  be  used  to  realise  a  solution 
for  the  multi-vehicle  SLAM  problem.  This  solution  can  be  implemented  as  an  iterative  scheme, 
alternating  between  a  bank  of  parallel  filters  for  the  landmarks,  and  a  bank  of  parallel  filters  for  the 
sensor  platforms.  For  the  application  considered,  there  is  only  a  single  platform,  and  the  PMHT 
algorithm  is  realised  with  Mpov  +  1  independent  EKFs. 

The  remainder  of  the  report  is  set  out  as  follows.  Section  2  defines  the  multi-vehicle  SLAM 
problem,  and  summarises  the  stacked  state  vector  and  FastSLAM  methods  for  solving  it.  Section  3 
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derives  a  PMHT  algorithm  for  multi-vehicle  SLAM.  Section  4  compares  the  PMHT  algorithm  with 
the  stacked  state  vector  EKF  and  FastSLAM  using  simulations.  The  PMHT  algorithm  is  applied 
to  a  benchmark  data  set  recorded  in  Victoria  Park  in  section  5.  Section  6  presents  a  summary. 
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2  Problem  Definition 


Consider  the  multi-vehicle  SLAM  problem  where  there  are  N  sensors  observing  M  land¬ 
marks.  Not  all  landmarks  are  necessarily  visible  to  all  sensors  at  any  particular  time.  At  discrete 
epochs,  the  sensors  observe  their  environment,  but  the  time  between  epochs  is  not  necessarily  reg¬ 
ular,  and  the  sensors  may  or  may  not  make  observations  at  the  same  time.  The  PMHT  is  derived 
over  a  batch  of  data,  but  it  can  be  implemented  as  a  sliding  window,  or  a  recursive  estimator,  de¬ 
pending  on  the  requirements  of  the  application.  Each  of  these  latter  forms  is  simply  obtained  from 
the  batch  processor,  and  so  a  batch  will  be  assumed. 

Suppose  that  there  are  T  observation  times  (scans).  The  time  of  the  tth  scan  will  be  denoted 
Tt  and  the  number  of  observations  from  sensor  n  as  r)f.  rff  may  be  zero.  Let  ztnr  denote  the  rth 
observation  from  sensor  n  at  scan  t ,  and  Ztn  be  the  set  of  all  observations  from  sensor  n  at  scan  t. 
If  sensor  n  does  not  collect  observations  at  time  rt ,  then  rft  —  0  and  Ztn  =  0,  the  empty  set.  Zt  is 
the  set  of  all  measurements  at  scan  t ,  and  Z  is  the  grand  collection  of  all  of  the  observed  data. 

Let  the  state  of  sensor  n  at  scan  t  be  denoted  by  yf.  The  set  of  states  for  all  scans  is  denoted  as 
Y72,  and  the  set  of  all  sensor  states  as  Y.  Similarly,  let  the  state  of  landmark  m  at  scan  t  be  denoted 
by  x™.  The  set  of  landmark  states  for  all  scans  is  denoted  as  Xm,  and  the  set  of  all  landmark  states 
as  X. 

Let  ktnr  denote  the  true  source  of  measurement  z*nr.  The  index  ktnr  gives  the  landmark  that 
gave  rise  to  measurement  ztnr,  thus  ktnr  El...  M.  ktnr  is  referred  to  as  the  assignment  index 
and  is  unknown  (or  else  data  association  is  unnecessary).  The  sets  K K*,  and  K  are  defined  as 
above. 

Assume  that  the  prior  distribution  of  the  state  of  each  sensor  is  known,  and  is  given  by  0^  (yJJ ) 
for  sensor  n.  Similarly,  the  known  prior  distribution  of  each  landmark  is  given  by  ^  (x™)  for 
landmark  m.  If  these  are  unknown,  then  it  is  assumed  that  an  estimation  scheme  exists  based  on 
measurements  (a  la  track  initiation). 

The  sensor  dynamics  are  described  by  the  evolution  pdf  (yt'lyt-i)^  and  the  landmark  dy¬ 
namics  by  ipY1  (^T'\xt^i)  •  Both  of  these  are  assumed  known. 

Finally,  the  observation  process  is  described  by  a  known  measurement  pdf  that  is  denoted  as 
Qt  ( ztnr\yt  ?  ktnr  =  wi).  This  pdf  may  be  different  for  each  sensor. 


2.1  Stacked  State  Vector  Solution 


The  standard  Kalman  Filter  solution  is  based  on  stacking  the  state  vectors  of  the  various  un¬ 
known  objects.  This  approach  was  first  introduced  in  [Smith  et  al.  1990]. 

The  problem  described  above  contains  M  unknown  landmark  state  vectors  and  N  unknown 
sensor  positions.  Let  the  system  state  be  defined  as 


-‘t  — 
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That  is,  at  is  the  stacked  vector  of  all  the  states  at  scan  t. 
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Assume  that  the  assignments  are  known,  and  that  the  random  processes  are  linear  and  Gaus¬ 
sian.  That  is, 


«(y S) 

(2) 
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~  A  (Hf  +  Ht  xt  ,  Rt ) , 

(6) 

where  N(p,  X)  is  a  normal  density  function  with  mean  /i  and  covariance  matrix  X. 
The  evolution  pdf  for  the  system  state  is  then  also  linear  and  Gaussian,  given  by 

p(Et\Et~i)  ~  N  (FtEt-i,  Qt ) , 

where  Tt  and  Qt  are  block  diagonal  matrices,  e.g. 

Ff 1  0  ...  0 

o 


(7) 


Tt  = 


zy,N 


0 


0 

-x.l 


Similarly  the  system  prior  is  given  by 

p(Eq)  ~  N  (EqjVo)  , 


(8) 


where  So  is  the  stacked  vector  of  the  prior  means,  and  Vo  is  the  block  diagonal  prior  covariance 
matrix. 


The  mean  of  the  measurement  pdf  for  measurement  ztnr  can  be  written  as 

uV»«  n  I  \Axin~jn  m  77 
''t  Vt  r  Xj.  —  ntnr'-'ti 

where  H tnr  is  a  sparse  matrix  with  Hf'T1  and  H*’n  positioned  appropriately 

Htrar  =  [•  •  •  0 . . .  W\'n  . . .  0 . . .  Hj . . .  0 . . .] . 

A  stacked  measurement  matrix  can  now  be  constructed 

Htu 


(9) 


Ht  = 
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41  Vt 
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(10) 
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_  T  T  T  ^  T 

Ztll  5  •  •  •  5  Zf21  5  •  •  • 


and  a  stacked  measurement  vector 

Zt  = 

so  that  the  probability  of  all  of  the  measurements  in  scan  t  is 

P  (Zt l Yt,  Xt,  K t)  =  p  (Zt\Eu  Kt)  ~  N  {Ht'Bu  Kt) 


(11) 


(12) 


where  7 Zt  is  a  block  diagonal  matrix  made  up  of  the  covariances  corresponding  to  each  individual 
measurement. 

It  should  now  be  clear  that  a  Kalman  Filter  can  be  implemented  to  estimate  Et  from  Zt.  Under 
the  linear  Gaussian  conditions,  this  will  be  an  optimal  estimator. 

The  primary  difficulty  with  this  approach  is  that  the  solution  does  not  scale  well  with  the  prob¬ 
lem  size.  The  length  of  the  system  state  vector,  S,  grows  linearly  with  the  number  of  landmarks, 
M.  This  means  that  the  covariance  matrix  used  by  the  Kalman  Filter  will  grow  as  M2.  The  sparse¬ 
ness  of  the  matrices  Tt  mdQt  can  be  exploited  to  reduce  the  required  computation  overhead,  but 
the  system  covariance  matrix  will  not  be  sparse  because  the  measurement  process  couples  the 
states.  For  problems  with  a  large  number  of  landmarks,  this  approach  becomes  infeasible. 


2.1.1  Data  Association 

The  above  analysis  assumed  that  the  assignment  of  measurements  to  landmarks  was  known, 
i.e.  ktnr  is  known.  This  will  usually  not  be  true.  To  relax  this  assumption,  an  association  process 
such  as  global  nearest  neighbours  can  be  used.  The  algorithm  requires  hard  assignments,  so  it  may 
perform  poorly  in  ambiguous  situations. 


2.1.2  Nonlinearity 

The  obvious  way  to  extend  the  Kalman  Filter  approach  for  nonlinear  problems  is  to  use  an 
Extended  Kalman  Filter.  This  replaces  the  true  state  evolution  and  measurement  functions  with 
linear  approximations  based  on  a  truncated  Taylor  series  expansion.  The  matrices  Tt  and  Tit  are 
then  formed  using  Jacobians.  The  EKF  is  known  to  perform  poorly  for  highly  nonlinear  systems, 
and  may  diverge. 

Alternatively,  the  stacked  state  vector  problem  could  be  tackled  with  a  nonlinear  filter,  such  as 
a  particle  filter.  However,  particle  filters  are  not  well  suited  to  problems  with  a  high  dimensional 
state,  because  one  needs  to  use  a  large  number  of  particles  to  populate  the  state  space. 


2.1.3  Large  Maps 

When  the  number  of  landmarks  is  large,  the  Kalman  Filter  approach  is  infeasible.  One  solution 
is  to  only  update  a  subset  of  the  landmarks  in  the  locale  of  the  sensor  [Guivant  &  Nebot  2001a]. 
When  the  sensor  moves  to  a  new  locale,  the  measurements  not  included  for  these  landmarks  can 
be  lumped  together  and  the  map  corrected. 
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2.2  FastSLAM 

The  FastSLAM2  approach  of  [Montemerlo  2003]  is  based  on  the  exact  Bayesian  factorisation 
of  the  problem. 


M 

P(X,  Y|Z)  =P(Y|Z)  P(Xm|Y,Z).  (13) 

m=l 

The  landmark  terms  on  the  right  hand  side  can  each  be  solved  with  independent  EKFs,  but  the 
sensor  term  is  more  difficult.  FastSLAM  uses  a  particle  filter  approach  to  approximate  this.  To  the 
author’s  knowledge,  FastSLAM  has  only  been  applied  to  single  platform  problems. 

There  are  two  published  versions  of  the  FastSLAM  algorithm,  referred  to  as  FastSLAM L0 
[Montemerlo  et  al.  2002]  and  FastSLAM2.0  [Montemerlo  et  al.  2003]  respectively.  These  algo¬ 
rithms  are  substantially  the  same,  except  that  the  FastSLAM2.0  uses  a  different  proposal  distri¬ 
bution.  Under  FastSLAM2.0,  each  particle  does  more  work,  but  the  algorithm  can  get  by  with 
fewer  particles.  In  the  limit,  as  the  number  of  particles  becomes  large,  the  two  are  equivalent. 
[Montemerlo  2003]  gives  a  comparison  of  the  two  variants,  and  demonstrates  under  which  condi¬ 
tions  one  is  preferred  over  the  other. 

2.2.1  Data  Association 

FastSLAM  naturally  incorporates  probabilistic  data  association.  It  does  this  by  sampling  the 
possible  association  combinations  with  the  particles.  So,  the  collection  of  particles  spans  the  set  of 
assignment  hypotheses,  and  it  is  hoped  that  the  particles  that  survive  are  those  carrying  the  correct 
hypotheses. 

2.2.2  Nonlinearity 

The  heart  of  FastSLAM  is  a  particle  filter  for  estimating  the  platform  state.  The  particle  filter 
is  a  natural  choice  for  nonlinear  problems,  and  it  should  be  anticipated  that  FastSLAM  would  offer 
an  advantage  in  this  type  of  problem. 

2.2.3  Large  Maps 

The  landmark  estimates  are  conditional  estimates  given  the  platform  state.  Since  each  particle 
has  (at  least  potentially)  a  different  state  value,  each  particle  must  carry  out  its  own  landmark  esti¬ 
mation.  However,  since  the  landmarks  are  not  dynamic,  there  is  no  prediction,  and  the  conditional 
density  only  changes  when  a  landmark  is  observed,  so  the  algorithm  needs  to  do  Mp  x  Mpov 
landmark  updates,  where  Mp  is  the  number  of  particles  and  Mpov  is  the  number  of  landmarks  in 
the  field  of  view.  This  means  that  there  are  M  —  Mpov  landmarks  that  are  not  updated  for  each 
particle. 

After  the  update,  each  particle  has  a  weight,  and  the  weighted  combination  of  the  particles  is 
the  pdf  estimate.  It  turns  out  that  particles  will  become  degenerate  and  have  zero  weight  unless 

2This  section  will  have  to  be  evolutionary  as  my  understanding  of  this  algorithm  improves 
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resampling  is  done.  In  resampling,  the  new  set  of  particles  is  drawn  from  the  updated  set  using  the 
weights  as  a  pmf  for  the  selection.  This  means  that  the  filter  has  to  do  Mp  particle  copies  during 
resampling.  Results  presented  in  [Montemerlo  2003]  demonstrate  that  the  particle  copy  process 
actually  limits  the  performance  of  the  algorithm  since  each  particle  contains  M  state  estimates.  It 
turns  out  that  many  of  the  landmark  estimates  that  are  out  of  the  current  field  of  view  will  be  the 
same  because  the  resampling  causes  the  higher  weight  particles  to  be  selected  more  frequently. 
This  means  that  some  of  the  resampling  copy  process  is  creating  duplicate  copies  of  the  same  data 
which  is  wasteful  of  memory  and  computation.  By  using  pointers  to  trees  of  landmark  estimates, 
FastSLAM  has  an  efficient  method  for  handling  these  out  of  sight  landmarks.  [Montemerlo  2003] 
argues  that  the  complexity  of  this  scheme  is  logarithmic  in  M  since  the  binary  tree  used  to  store 
landmark  estimates  has  depth  log2  M.  So,  assuming  that  Mpov  M,  then  the  computation  time 
of  the  algorithm  is  limited  by  Mp  log2  M. 

Arriving  at  a  complexity  only  logarithmic  in  the  number  of  landmarks  seems  remarkable  (less 
than  linear!)  but  this  conclusion  is  a  little  deceptive.  The  algorithm  must  do  Mp  particle  updates 
each  of  which  consists  of  a  platform  prediction  (requiring  only  a  random  number  draw),  Mpov 
local  landmark  updates,  a  weight  calculation,  and  M  resamplings,  each  of  which  is  heuristically 
log2  M  in  complexity.  In  comparison  the  stacked  state  vector  using  only  local  landmarks  needs  to 
assign  measurements  and  perform  a  Mpov  stacked  state  EKF.  The  particle  filter  weight  calcula¬ 
tion  requires  Mp  *  Mpov  pdf  evaluations,  each  of  which  is  expensive.  Even  considering  only  the 
landmark  updates,  the  particle  filter  uses  Mp  *  Mpov  EKFs  whereas  the  stacked  EKF  uses  one  of 
size  Mpov •  Although  the  single  stacked  EKF  is  costly,  FastSLAM  does  numerous  small  filters. 
Overall,  the  stacked  state  vector  on  the  local  area  is  independent  of  M  except  on  global  updates, 
and  FastSLAM  is  not.  So,  it  appears  that  the  low  complexity  claims  of  FastSLAM  are  overstated. 
However,  FastSLAM  does  have  advantages  for  data  association  and  nonlinear  problems. 
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3  Probabilistic  Multi-Hypothesis  Tracker  for  SLAM 

The  fundamental  philosophy  behind  the  PMHT  approach  to  SLAM  presented  here  is  an  itera¬ 
tive  application  of  partial  Expectation  Maximisation  (EM).  This  will  be  explained  by  first  briefly 
explaining  EM  for  the  simpler  tracking  problem,  where  Y  is  known  (i.e.  the  standard  PMHT). 

The  task  is  to  estimate  the  target  states,  X,  given  the  observed  data  Z.  However,  the  assignment 
of  the  measurements,  K,  is  unknown.  In  an  EM  context,  the  assignments  are  treated  as  missing 
data  or  nuisance  parameters ;  it  is  unimportant  what  K  are,  except  that  it  appears  one  must  solve 
for  the  joint  posterior  P(X,  K|Z)  to  obtain  the  optimal  X.  EM  theory  applied  to  this  problem 
defines  an  auxiliary  function 

Q(X|X)  =  ^P(K|X,Z)logP(X,K,Z)  (14) 

K 

that  is  the  expectation  of  the  joint  likelihood  over  the  assignments.  The  probability  of  the  assign¬ 
ments  is  evaluated  at  some  guess  of  the  states,  X.  One  finds  the  state  X  that  maximises  Q(X|X) 
and  this  becomes  an  improved  guess  of  the  states.  EM  theory  states  that  the  iterative  application 
of  this  process  converges  to  the  same  X  that  optimises  P(X,  K|Z),  subject  to  local  maxima.  This 
is  useful  if  Q(X|X)  is  easy  to  maximise,  which  it  is  for  the  tracking  case. 

In  the  case  of  SLAM,  the  problem  has  an  additional  unknown,  Y,  the  sensor  states.  So  the 
direct  application  of  PMHT  would  require  the  joint  estimation  of  X  and  Y,  as  with  the  Kalman 
Filter  method.  However,  if  partial  EM  is  used  instead,  the  problem  is  much  simpler.  Partial  EM  is 
where  one  seeks  not  to  estimate  all  of  the  states,  but  to  estimate  some  of  them,  and  keep  the  others 
fixed.  If  Y  is  kept  fixed,  it  is  equivalent  to  assuming  that  the  current  estimated  sensor  trajectory  is 
the  true  trajectory,  and  then  doing  the  standard  tracking  problem.  One  iteration  of  the  EM  for  this 
would  be 

•  Calculate  the  probability  of  the  assignments  given  the  assumed  sensor  trajectory,  Y,  and  the 
current  landmark  state  estimates  X^_i.  This  defines  the  auxiliary  function. 

•  Determine  a  new  landmark  state  estimate  Xz  by  optimising  the  auxiliary  function.  This  can 
be  done  in  parallel  for  each  landmark  -  the  auxiliary  function  contains  independent  terms. 

A  similar  EM  process  can  be  obtained  by  only  updating  the  sensor  states.  Here  the  landmark 
positions  are  assumed  known.  For  this  case,  the  sensor  states  can  be  independently  estimated  by  a 
bank  of  filters.  By  combining  the  two  partial  updates,  an  iterative  scheme  is  achieved  which  will 
converge,  subject  to  local  maxima.  The  algorithm  is  thus  of  the  form 

•  Choose  an  initial  guess  of  the  landmark  positions,  Xq,  and  the  sensor  trajectories,  Yo- 

•  Calculate  the  assignment  probabilities  to  determine  the  auxiliary  function. 

•  Refine  the  sensor  trajectory  estimates  to  find  Y*  by  maximising  the  auxiliary  function  with 
landmarks  fixed  at  X  =  Xi_i. 

•  Calculate  the  assignment  probabilities  to  determine  the  auxiliary  function. 

•  Refine  the  landmark  estimates  to  find  X;  by  maximising  the  auxiliary  function  with  sensors 
fixed  at  Y  =  Y;_i. 

•  Repeat  2  ...  5  until  convergence. 


9 


DSTO-TR-1691 


3.1  The  Algorithm 


Assume  that  all  state  sequences  are  independent  of  each  other,  and  all  measurements  are  con¬ 
ditionally  independent,  given  the  relevant  states.  Assume  that  all  measurements  from  the  same 
sensor  are  identically  distributed  given  the  relevant  states.  Assume  that  the  assignments  are  ILD 
random  variables  from  a  prior  distribution  7r™  =  P{ktr  =  rn). 

Due  to  the  independence  assumptions,  the  complete  data  likelihood  is  given  by: 


where, 


P(X,  Y,  K,  Z)  =  P(X)P(Y)P(K)P(Z|X,  Y,  K) 


P(Y) 
P(X) 
P(K) 
P(Z|X,  Y,  K) 
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(16) 
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(18) 

(19) 


So, 


logP(X,  Y,  K,  Z)  =  log  P(X)  +  log P(Y)  +  logP(K)  +  logP(Z|X,  Y,  K)  (20) 


where  each  of  the  terms  to  the  right  above  is  itself  a  sum  of  terms  due  to  the  factorised  nature  of 
the  problem. 


The  assignments  are  missing  data,  so  the  EM  auxiliary  function  requires  P(K|X,  Y,  Z)  which 
can  be  found  using  Bayes’  Rule  and  some  simple  manipulations: 


P(K|X,  Y,  Z) 
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t-lr-1  Y,*r<?(ztr\y?,xr) 


rn  1 

T  nt 

—  fj  wktrtr 
t=  1  r=l 


(21) 


(22) 


So,  the  conditional  probability  of  the  assignments  is  given  by  the  product  of  individual  per 
measurement  terms.  In  common  PMHT  parlance,  the  wmtr  is  referred  to  as  the  weight  for  mea¬ 
surement  ztr  and  target  model  m.  Notice  that  this  weight  is  simply  a  likelihood  ratio  evaluated 
using  the  current  state  estimates. 

Combining  all  this  leads  to  the  auxiliary  function 

<?(X,Y|Xi>Yi)  =  ^P(K|X,Y,Z)logP(X,Y,K,Z) 

K 

T  M  nt 

=  log  P(X)  +  log  P(Y)  +  J2J2J2 loS  <L 

t=  1  m=  1  r=  1 

T  M  nt 

+  ^2^2  ^2  Wmtr  log  C t  ( ztr \Vt  ,  XT)  (23) 

t=  1  m= 1  r— 1 


Notice  that  the  measurement  term  is  composed  of  pairs  of  states,  each  with  one  sensor  and 
one  target,  coupled  together  by  the  measurement  function.  Thus  (at  worst)  all  the  targets  and  all 
the  sensors  are  coupled  together  through  this  term.  However,  none  of  the  targets  are  coupled  with 
each  other,  and  none  of  the  sensors  are  coupled  with  each  other.  This  is  pretty  much  the  standard 
PMHT  result  which  allows  the  normal  algorithm  to  be  achieved  with  linear  complexity  in  the 
number  of  targets.  So,  if  X  were  known,  Y  could  be  estimated  with  independent  filters  for  each 
sensor.  Similarly,  if  Y  were  known,  X  could  be  estimated  with  independent  filters  for  each  target. 
These  are  the  standard  navigation  and  tracking  (mapping)  functions  respectively.  This  means  the 
problem  is  amenable  to  partial  EM. 


3.2  Statement  of  Algorithm 

The  PMHT  can  be  implemented  either  as  a  batch  algorithm,  or  recursively.  Here  the  batch 
form  is  presented.  The  recursive  algorithm  is  trivially  recovered  by  choosing  a  unit  length  batch. 

1.  Initialise  the  target  and  sensor  states,  X(0)  and  Y (0). 

2.  Calculate  the  assignment  weights,  w^r 

3.  Refine  the  sensor  estimates,  Y(i),  assuming  the  target  states  are  fixed  (ie  X  =  X(i  —  1)). 
This  is  independent  for  the  sensors;  N  parallel  filters/smoothers. 

4.  Recalculate  the  assignment  weights,  uWr 
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5.  Refine  the  target  estimates,  X(i),  assuming  the  target  states  are  fixed  (ie  Y  =  Y (i)).  This 
is  independent  for  the  targets;  M  parallel  filters/smoothers. 

6.  Return  to  step  2  until  convergence. 

In  theory,  convergence  should  be  checked  by  evaluating  the  auxiliary  function  and  monitoring 
its  growth  (each  step  should  increase  it  by  an  asymptotically  smaller  amount).  However,  in  practice 
calculating  the  auxiliary  function  is  more  expensive  than  the  rest  of  the  algorithm,  and  most  of  the 
time  it  converges  in  only  two  or  three  iterations.  So,  a  practical  scheme  is  likely  to  rather  run  the 
EM  process  for  a  fixed  number  of  iterations  (perhaps  three). 

Under  linear  Gaussian  statistics,  the  state  estimates  can  all  be  determined  with  Kalman  Fil¬ 
ters/Smoothers.  The  sensor  filters  can  be  implemented  using  synthetic  measurements  which  are 
effectively  a  mean  pseudo-measurement,  combining  the  various  observations,  with  an  associated 
synthetic  covariance. 


3.3  Data  Association 

PMHT  is  inherently  a  data  association  algorithm.  The  algorithm  assumes  that  the  data  asso¬ 
ciation  is  unknown  and  uses  EM  to  treat  it  as  nuisance  parameters.  The  resulting  algorithm  is 
somewhat  like  a  probabilistic  data  association  approach. 


3.4  Nonlinearity 

PMHT  is  not  restricted  to  a  particular  estimation  algorithm.  In  the  general  statement  of  the 
algorithm,  it  simply  requires  that  the  implementation  use  the  optimal  estimator  for  the  problem. 
For  nonlinear  problems,  an  EKF  could  be  used,  or  a  particle  filter  could  be  implemented.  Both 
forms  have  been  used  by  various  authors,  and  the  choice  depends  on  what  estimator  is  appropriate 
for  the  problem. 

So  far,  the  example  problem  presented  in  the  next  section  has  assumed  that  the  state  estimation 
can  be  adequately  solved  using  Extended  Kalman  Filters/Smoothers.  Further  work  is  required  to 
obtain  an  analytic  result  demonstrating  that  this  is  indeed  the  linear  approximate  solution  -  or  to 
determine  what  is,  if  something  else. 


3.5  Large  Maps 

The  PMHT  approach  is  well  suited  to  large  maps.  Due  to  the  conditional  factorisation,  there 
is  no  need  to  update  the  estimates  of  landmarks  outside  of  the  field  of  view.  The  complexity  in  the 
number  of  landmarks  inside  the  field  of  view  is  linear,  so  the  total  cost  of  the  algorithm  comparable 
to  FastSFAM  with  a  single  particle,  and  much  less  than  that  of  the  stacked  state  vector  Kalman 
Filter. 
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4  Comparison  of  PMHT  SLAM  with  Other  Approaches 

The  previous  section  derived  a  PMHT  based  algorithm  for  solving  the  SLAM  problem.  As 
stated  earlier,  this  is  far  from  the  first  solution  to  SLAM,  so  it  is  important  to  compare  it  with  ex¬ 
isting  approaches  to  the  problem.  Three  metrics  will  be  considered  for  this  comparison:  platform 
position  estimation  accuracy,  computation  time  requirement,  and  data  storage  requirement.  The 
first  measures  the  quality  of  the  algorithm  output,  and  the  others  measure  the  cost  of  using  the 
algorithm.  The  performance  of  the  PMHT  will  be  compared  with  the  stacked  state  vector  EKF 
approach  and  FastSLAM.  In  the  EKF  case,  implementation  is  relatively  simple,  and  quantitative 
results  are  readily  obtained.  However,  implementation  of  FastSLAM  is  more  difficult.  To  avoid 
erroneous  results  due  to  poor  implementation,  inferences  about  performance  compared  with  Fast¬ 
SLAM  will  be  made  using  the  results  comparing  FastSLAM  performance  and  EKF  performance 
given  in  [Montemerlo  2003]. 


4.1  Estimation  Accuracy 


To  measure  estimation  accuracy,  it  is  necessary  to  compare  the  algorithm  output  with  truth. 
For  the  real  experiment  in  section  5,  the  only  truth  available  is  a  set  of  GPS  reports,  that  fluctuate 
by  several  metres  between  reports.  This  fluctuation  is  comparable,  if  not  greater  than  the  SLAM 
output  error,  so  this  data  does  not  provide  a  useful  set  for  quantitative  error  analysis.  In  order  to 
get  reliable  error  statistics,  simulated  data  was  be  used. 


The  simulated  scenario  contained  a  single  platform  that  observed  landmarks  using  a  radial 
sensor  (range  and  bearing).  The  accuracy  of  this  sensor  was  varied  by  scaling  a  base  measurement 
variance  according  to 


R(k)  =  K? 


0.12  0 

0  0.052 


(24) 


When  k  was  large,  then  data  association  became  important. 


The  platform  state  was  the  same  as  was  used  for  the  Victoria  Park  experiment  described  later. 
Namely, 


X  position 
Y  position 
heading 
speed 

steering  angle 


'  t 


(25) 


with  white  Gaussian  process  noise  on  the  speed  and  steering  angle  to  emulate  manoeuvres.  The 
platform  is  initialised  at  the  origin  with  zero  heading,  a  speed  of  n/2  and  a  steering  angle  of 
tan_1(L/50),  where  L  is  the  axle  length  of  the  vehicle  from  the  Victoria  Park  experiment,  i.e. 
2.83  metres.  This  corresponds  to  a  50  metre  radius  turn  at  one  revolution  per  200  seconds.  The 
state  evolution  equations  for  this  model  are  given  in  (36)  . . .  (40).  Two  process  noise  examples 
were  considered.  The  low  process  noise  case  had  no  process  noise  on  the  speed  and  noise  with 
a  standard  deviation  of  0.0001  radians  on  the  steering  angle.  This  is  effectively  no  process  noise. 
The  high  process  noise  case  had  standard  deviations  of  0.04  metres  per  second  and  0.02  radians 
respectively. 
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(a)  Example  platform  trajectories 


Figure  1:  Simulation  examples 


A  fixed  number  of  landmarks  was  randomly  placed  inside  the  world  volume.  To  avoid  extreme 
non-linearity,  any  landmark  that  was  within  three  metres  of  the  vehicle  path  was  removed.  The 
scenario  runs  for  200  sensor  updates  at  the  same  rate  as  the  laser  sensor  in  the  Victoria  Park 
experiment,  approximately  4.7  Hertz. 

Figure  1(a)  shows  a  few  realisations  of  the  high  process  noise  example.  The  trajectory  with 
zero  process  noise  is  shown  as  a  dashed  line.  This  demonstrates  that  the  process  noise  is  indeed 
high,  but  the  true  trajectories  are  still  smooth  due  to  the  integration  effect  of  the  state  model. 
Figure  1(b)  shows  an  example  trial  with  low  process  noise.  The  sensor  field-of-view  is  shown  as  a 
semi-circle.  Due  to  the  random  landmark  placement,  some  landmarks  are  quite  close  together. 

Monte  Carlo  trials  were  run  for  varying  sensor  accuracy  (ac)  and  the  two  process  noise  values. 
Estimation  performance  was  measured  using  two  quantities 


Divergent  track  rate  is  the  percentage  of  trials  on  which  the  worst  estimation  error  is  more  than 
3  metres. 

RMS  estimation  error  is  averaged  over  time  and  trials  for  non-divergent  trials. 


Figure  2  shows  the  RMS  estimation  error  for  the  two  algorithms  as  a  function  of  the  mea¬ 
surement  accuracy.  As  expected,  the  error  is  higher  for  the  high  process  noise  case.  For  the  high 
process  noise  and  high  measurement  error  case,  both  the  PMHT  and  the  EKF  give  similar  error 
performance,  but  for  all  other  cases,  the  PMHT  has  lower  error. 

Table  1  gives  the  percentage  of  divergent  platform  estimates.  The  PMHT  is  clearly  far  superior 
to  the  EKF  in  this  area.  In  fact,  the  PMHT  only  ever  diverges  for  the  most  difficult  scenario  where 
process  and  measurement  noise  are  both  high. 

From  these  results,  it  is  clear  that  the  PMHT  algorithm  gives  much  better  estimation  perfor¬ 
mance  than  the  EKF.  This  is  primarily  because  of  data  association.  The  EKF  here  uses  a  nearest 
neighbour  based  data  association  method.  The  EKF  has  relatively  poor  performance  for  the  low¬ 
est  measurement  noise  case  (k  =  1).  This  is  because  the  low  measurement  noise  results  in  a  high 
Kalman  gain,  and  when  incorrect  assignments  are  made,  the  spurious  measurement  correction 
causes  the  platform  estimate  to  diverge. 
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|  — I —  EKF  low  Q  ■  EKF  high  Q  -  +  -  PMHT  low  Q  ■  PMHT  high  Q 


Figure  2:  RMS  position  estimation  error 
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Table  1:  Percentage  of  divergen  t  tracks 
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No  attempt  was  made  to  implement  FastSLAM,  so  there  are  no  simulation  results.  However, 
it  is  assumed  that  it  would  have  been  able  to  achieve  similar  RMS  and  divergence  performance 
as  the  PMHT,  if  enough  particles  were  used.  Since  FastSLAM  uses  a  sampling  approach  to  data 
association,  the  number  of  particles  would  need  to  be  high  enough  to  at  least  enumerate  all  proba¬ 
ble  association  hypotheses,  which  in  the  more  challenging  parameter  combinations  was  very  high. 
The  cost  of  using  many  particles  is  explored  in  the  following  sections. 


4.2  Computation  Overheads 

The  computation  requirements  of  the  algorithm  were  also  tested  using  simulated  data.  The 
scenario  was  similar  to  that  used  above,  except  that  the  process  noise  was  set  to  zero  for  both 
speed  and  steering  angle.  This  meant  that  the  true  platform  trajectory  was  always  a  circular  orbit.  A 
longer  duration  was  used:  one  thousand  sensor  measurements  were  simulated,  which  corresponded 
to  slightly  more  than  one  circuit  of  the  orbit.  In  the  accuracy  experiment,  the  landmarks  were 
randomly  placed,  but  here  they  were  positioned  in  a  regular  grid  to  make  it  easier  to  control 
the  landmark  density.  The  sensor  measurement  noise  was  made  low  (k  =  1)  to  simplify  data 
association.  Since  the  process  noise  was  zero,  the  Kalman  gain  was  also  always  zero,  with  the 
result  that  the  state  estimate  was  simply  predicted  from  the  starting  point.  This  was  an  unrealistic 
scenario,  but  it  doesn't  change  the  computation  resource  required  for  the  problem. 

The  cpu  time  elapsed  for  a  single  scan  update  was  recorded  for  every  scan  and  over  a  number 
of  Monte  Carlo  trials.  This  time  was  measured  as  a  function  of  the  number  of  landmarks  in  the 
sensor  field-of-view,  and  the  total  number  of  landmarks  in  the  map.  The  number  of  landmarks  in 
the  field-of-view  changes  from  scan  to  scan  depending  on  the  sensor  position  and  orientation,  and 
was  varied  by  running  trials  with  different  landmark  densities.  For  a  particular  landmark  density, 
the  number  of  landmarks  in  the  map  was  varied  by  simply  using  a  proportionally  larger  map. 
When  the  map  size  was  very  large,  most  of  the  landmarks  were  never  observed.  However,  the 
SLAM  algorithm  is  initialised  with  an  estimate  for  each  landmark,  so  it  is  functionally  equivalent 
to  a  scenario  where  the  sensor  has  observed  every  landmark  at  some  past  time. 

The  computation  requirements  of  each  of  the  PMHT  SLAM  steps  are  now  discussed  to  pro¬ 
duce  an  expected  complexity  for  the  algorithm. 


Initialisation  Before  measurements  are  processed,  all  states  are  initialised.  This  is  linearly  depen¬ 
dent  on  the  total  map  size,  but  very  cheap,  and  not  expected  to  impact  on  overall  computation 
cost.  For  each  scan,  the  sensor  state  is  initialised  by  predicting  ahead  from  the  previous  time. 
This  is  independent  of  the  map  size,  and  also  cheap. 

Gate  landmarks  The  simplest  realisation  of  this  process  is  to  measure  the  range  and  angle  to 
each  landmark,  using  current  estimates,  and  test  whether  these  are  within  the  sensor  field  of 
view.  This  is  linear  in  the  number  of  landmarks. 

Calculate  weights  The  most  costly  part  of  the  weight  calculation  is  evaluating  the  measurement 
density  terms.  This  cost  is  reduced  by  gating  the  measurements.  However,  the  number  of 
gated  measurements  is  dependent  on  the  landmark  density.  This  will  be  only  slightly  more 
than  linear  in  the  number  of  landmarks  in  the  field  of  view,  provided  that  the  number  of 
gated  measurements  is  small. 
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Figure  3:  Computation  time  as  a  function  of  map  size 


Update  platform  state  The  most  costly  part  of  the  platform  update  is  incorporating  the  sensor 
measurements.  By  using  a  composite  measurement  approach,  this  is  linear  in  the  number  of 
landmarks. 

Update  landmark  state  Each  landmark  EKF  is  independent  of  the  number  of  landmarks,  except 
through  the  influence  this  has  on  the  number  of  gated  measurements.  Again,  provided  this 
is  small,  the  landmark  update  is  linear  in  the  number  of  landmarks  in  the  field  of  view. 


Most  of  the  above  steps  are  independent  of  the  number  of  total  number  of  landmarks,  and  only 
linear  in  the  number  in  the  field  of  view.  The  exception  is  the  initial  field-of-view  gating,  which  is 
linear  in  the  total  map  size.  This  step  would  be  a  requirement  for  any  SLAM  approach;  to  remove 
it  makes  the  rest  of  the  algorithm  dependent  on  the  whole  map  size,  not  simply  the  number  in  the 
field-of-view. 

Figure  3  shows  how  the  processing  time  for  a  single  time  update  varies  as  a  function  of  the 
number  of  landmarks  in  the  field  of  view.  Each  curve  shows  a  particular  total  map  size,  M.  The 
cost  is  clearly  linear  in  the  number  of  landmarks  in  the  field  of  view.  There  is  also  an  offset 
between  the  curves  for  larger  total  map  sizes.  This  offset  is  linearly  dependent  on  the  map  size, 
and  is  due  to  the  field-of-view  gating.  Figure  4  shows  the  computation  time  excluding  that  used 
for  the  gating.  It  is  independent  of  the  total  map  size.  The  method  used  here  for  gating  is  a  simple 
direct  implementation,  and  methods  exist  to  improve  this.  One  example  might  be  to  group  the 
landmarks  into  tiles  and  to  gate  the  tiles  instead.  As  stated  earlier,  this  process  is  necessary  for  any 
algorithm,  and  so  optimising  it  is  not  explored  here. 
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Figure  4:  Computation  time  excluding  FOV  gating 


4.2.1  Comparison 

The  stacked  EKF  algorithm  will  scale  quadratically  with  the  total  map  size.  Using  local  region 
updates  as  in  [Guivant  &  Nebot  2001a],  this  is  reduced  to  quadratic  in  the  number  of  landmarks  in 
the  field-of-view  with  lower  rate  full  cost  updates.  In  either  case,  the  PMHT  algorithm  is  clearly 
quicker. 

The  full  algorithm  for  FastSLAM  is  laid  out  in  [Montemerlo  2003].  Simplistically,  it  com¬ 
prises  the  following  steps: 

1.  For  each  particle 

(a)  Sample  a  new  pose 

(b)  Calculate  the  likelihood  of  each  measurement-landmark  pair 

(c)  Choose  a  hard  data  association 

(d)  Update  features  according  to  data  association 

(e)  Calculate  particle  weight 

2.  Re-sample  particles 
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The  computation  cost  is  linearly  dependent  on  the  number  of  particles,  much  as  the  PMHT  cost 
is  linearly  dependent  on  the  number  of  EM  iterations  used.  In  [Montemerlo  2003],  the  computation 
cost  is  measured  for  a  fixed  landmark  density  and  a  varying  map  size.  It  shows  that  the  complexity 


DSTO-TR-1 69 1 


of  FastSLAM  is  logarithmic  in  the  map  size,  and  the  cause  of  this  cost  is  the  maintenance  of 
landmark  estimates  in  the  re-sampling  phase  of  the  particle  filter.  Thus,  the  dominant  cost  is  step  2 
from  above.  Notice  that  step  1(b)  involves  a  pdf  evaluation  for  every  landmark  and  measurement 
for  every  particle,  which  is  M  x  Mpov  x  Mp  if  Mpov  landmarks  are  detected  in  the  field- 
of-view  and  there  are  Mp  particles.  This  linear  complexity  in  M  is  not  apparent  in  the  results 
of  [Montemerlo  2003],  which  reflects  that  the  resampling  is  a  more  costly  exercise:  a  quadratic 
will  appear  linear  if  the  linear  coefficient  is  relatively  large.  The  dominant  cost  in  the  PMHT 
algorithm  for  large  maps  was  the  field-of-view  gating.  This  step  is  undertaken  to  reduce  the  cost 
of  likelihood  calculations  and  is  cheaper  than  1(b).  Since  step  1(b)  is  not  significant  in  the  overall 
FastSLAM  cost,  it  implies  that  the  whole  PMHT  algorithm  cost  is  insignificant  compared  with 
running  FastSLAM. 


4.3  Storage  Overheads 

The  data  storage  requirements  for  the  PMHT  algorithm  are  easy  to  determine.  Each  landmark 
in  the  filter  has  an  independent  estimator.  Assuming  that  the  state  vector  for  landmarks  has  length 
cLm  and  an  EKF  is  used,  then  each  landmark  requires  gJm  +  d?M  floating  points.  Similarly,  each 
platform  requires  d^j  +  d2N.  This  is  very  much  smaller  than  the  stacked  EKF,  which  would  require 
(Ndjsr  +  Mdjtf)  +  (Ndjsr  +  Mdjsi)2-  For  the  example  in  section  5,  there  are  nominally  150 
landmarks,  leading  to  a  difference  of  two  orders  of  magnitude.  For  a  two  dimensional  map  with 
M  AT,  the  stacked  state  vector  uses  approximately  M  times  more  memory. 

The  memory  usage  of  FastSLAM  is  presented  in  section  3.8  of  [Montemerlo  2003].  Here  Fast¬ 
SLAM  is  demonstrated  to  use  approximately  3  MB  of  memory  for  a  map  with  50,000  landmarks. 
For  a  map  this  size,  the  PMHT  SLAM  algorithm  would  require  approximately  2.3  MB,  around  the 
same  as  FastSLAM. 
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5  Victoria  Park  Data 


The  Victoria  Park  data  set  is  a  benchmark  SLAM  data  set  recorded  by  the  University  of  Sydney. 
In  the  experiment,  a  utility  was  fitted  with  various  sensors  and  driven  around  Victoria  Park  (at  the 
University  of  Sydney),  which  contains  sparse  trees.  A  laser  range  finder  was  used  to  observe  the 
trees,  and  inertial  measurements  of  the  vehicle’s  speed  and  steering  direction  were  collected.  GPS 
data  was  collected  to  provide  ground  truth.  Due  to  occlusion,  the  GPS  signal  was  not  available 
over  the  whole  experiment,  so  there  are  patches  where  no  truth  data  is  available.  Partial  occlusion 
also  caused  the  GPS  constellation  to  vary  with  platform  position.  Constellation  changes  cause 
jumps  in  the  GPS  position  estimate  of  up  to  five  metres.  This  makes  the  GPS  reports  useful  for 
gauging  gross  platform  localisation  performance,  but  not  for  quantifying  estimation  error.  The 
data  set  contains  over  seven  thousand  laser  measurements,  collected  over  approximately  half  an 
hour,  during  which  time  the  vehicle  moved  over  four  kilometres.  The  inertial  measurements  were 
collected  at  a  much  higher  rate  than  the  laser  measurements  (around  8.5  times  faster). 

Figure  5  shows  the  Victoria  Park  environment.  In  the  experiment,  the  trees  were  the  intended 
landmarks.  However,  other  objects  were  present,  and  were  detected  at  various  times.  Figure  5(a) 
is  an  image  taken  from  the  platform’s  perspective.  It  demonstrates  how  trees  close  to  the  sensor 
may  occlude  others  farther  away.  It  also  shows  some  of  the  non-tree  objects  in  the  environment, 
such  as  poles,  cars  and  pedestrians.  Figure  5(b)  shows  an  aerial  image  of  the  park.  It  highlights 
the  varying  density  of  the  trees  in  the  park,  and  also  environment  outside  the  park.  In  particular, 
there  is  a  road  to  the  South  East  of  the  park  that  has  numerous  moving  vehicles.  These  vehicles 
were  detected,  but  did  not  satisfy  the  stationary  target  assumption.  The  two  images  highlight  one 
of  the  major  difficulties  in  the  data  set:  track  maintenance  under  occlusion,  where  some  targets 
are  stationary  but  temporarily  hidden,  but  others  are  not  stationary.  Non-stationary  targets  will 
degrade  the  SLAM  performance,  since  it  assumes  all  visible  objects  are  stationary  landmarks. 
However,  discrimination  between  the  various  object  types  visible  to  the  sensor  is  non-trivial. 

Figure  6  contrasts  the  trajectory  obtained  purely  by  integrating  the  inertial  measurements  with 
the  GPS  reports.  This  highlights  the  inadequacy  of  the  inertial  measurements  as  an  assumed  truth, 
and  motivates  the  need  for  applying  a  SLAM  algorithm  to  this  data.  The  main  deficiency  in  the 
inertial  data  is  that  it  tends  to  over-estimate  turns.  Numerous  loops  can  be  seen  in  figure  6  that  are 
not  found  in  the  GPS  reports.  The  individual  GPS  reports  are  not  linked  with  a  line  because  of 
their  intermittent  nature;  putting  a  straight  line  through  the  gaps  would  not  properly  represent  the 
vehicle  trajectory. 


5.1  Vehicle  Geometry 


The  geometry  of  the  sensor  platform  is  illustrated  in  figure  7.  The  laser  range  finder  is  located 
on  the  front  left  side  of  the  car,  and  the  speed  measurement  is  made  at  the  rear  axle.  Relevant 
dimensions  are  indicated  in  the  diagram.  The  physical  location  of  the  sensors  on  board  the  platform 
is  of  importance  because  many  of  the  landmarks  are  observed  from  very  close  range,  at  times  less 
than  2  metres.  In  the  figure,  the  distance  between  the  car’s  axles  is  labelled  L,  the  width  of  the  car 
is  277,  and  the  dimensions  a  and  b  define  the  position  of  the  laser  sensor  relative  to  the  front  axle 
and  the  car’s  centreline  respectively. 
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(a)  Platform’s  perspective 


(b)  Aerial  perspective 

Figure  5:  The  Victoria  Park  Environment 
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(a)  Integrated  inertial  data  (b)  GPS  reports 

Figure  6:  Reliability  of  Inertial  Data 


Figure  7:  Vehicle  Geometry 
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5.2  Measurements 

There  are  two  sensors  available,  a  laser  range  finder  and  an  inertial  sensor.  The  inertial  sensor 
measures  the  vehicle  speed  and  steer  direction.  The  steer  direction  is  the  angle  between  the  front 
wheels  and  the  direction  vector  of  the  vehicle.  These  are  reported  in  metres  per  second  and  radians 
respectively.  Each  measurement  has  an  associated  time  stamp  measured  in  milliseconds,  and  the 
sensor  collects  a  measurement  every  twenty  five  milliseconds  (40  Hertz). 

The  laser  range  finder  provides  a  scan  of  361  angles  in  a  180  degree  arc  (i.e.  0.5  degree 
resolution)  in  front  of  the  vehicle.  For  each  angle,  the  range  to  the  nearest  reflector  is  reported.  If 
no  reflector  is  found  within  80  metres,  a  null  value  is  given.  This  data  is  also  accompanied  by  a  time 
stamp  in  milliseconds  and  the  sensor  scans  at  a  revisit  time  of  slightly  faster  than  214  milliseconds 
(about  4.7  Hertz).  Professor  Eduardo  Nebot  provided  a  detector  with  the  data  set.  This  detector 
takes  the  raw  laser  data  and  produces  reports,  each  giving  the  location  and  size  of  a  reflector.  The 
processed  laser  data  thus  consists  of  a  time  stamp,  and  a  set  of  observations  each  of  which  gives  a 
reflector’s  range  in  metres,  relative  bearing  in  radians,  and  estimated  size  in  metres.  The  detector 
is  able  to  estimate  the  reflector  size  because  the  sensor  resolution  is  high,  and  the  trees  are  visible 
in  several  azimuth  bins.  So  the  processed  laser  data  can  be  treated  as  measurements  from  a  polar 
sensor  that  also  measures  a  non-kinematic  attribute.  For  the  filter  implemented  here,  it  has  been 
chosen  to  ignore  the  size  measurement,  although  it  could  be  exploited  for  data  association. 


5.3  Models 


The  scenario  takes  place  in  a  two  dimensional  world  (ignoring  terrain  topography).  The  land¬ 
marks  are  modelled  as  stationary  objects,  so  their  state  vector  is  a  simple  two  dimensional  position 
vector 


J  X  position  l77 
\  Y  position  J 


(26) 


There  is  only  one  sensor  platform,  and  its  motion  is  approximated  using  a  constant  speed, 
constant  steering  angle  model 


X  position 
Y  position 
heading 
speed 

steering  angle 


/  t 


(27) 


with  white  Gaussian  process  noise  on  the  speed  and  steering  angle  to  account  for  manoeuvres. 
This  model  allows  for  measurement  noise  in  the  inertial  data  by  not  treating  them  as  simple  control 
inputs,  but  rather  observations  of  control  inputs.  Without  perturbations  (manoeuvres)  the  model 
reverts  to  a  circular  orbit  with  a  fixed  speed. 

It  is  assumed  that  there  is  no  slip  in  the  wheels  of  the  car,  which  is  an  acceptable  approximation 
for  moderate  dynamics,  and  gives  the  following  relations  for  the  state  evolution. 


y[  1]  =  y[4]cos(y[3]) 
y[  2]  =  2/ [4]  sin  (y[3]) 


(28) 

(29) 
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if[  3]  =  ^+an(y[5])  (30) 

Time  recursions  for  the  state  elements  are  found  by  integrating  the  instantaneous  heading  over 
the  revisit  interval 

Vt+ it1]  =  VtiM  +  tan(y  [5])  (sin  (l/t+lt3])  -  sin (yf[3])|  ,  (31) 

Vt+ it2)  =  Vd2}  +  tan(y  [5])  {cos  ~  cos  (l/i+i[3])}  i  (32) 

Vt+i  [3]  =  y,[ 3]  +  ^[4]t^[5])(rt+1-rt),  (33) 

2/t+i  t4]  =  yt[4]  +  w*[l],  (34) 

Vt+ 1[5]  =  yt[5]+w*[2].  (35) 

When  the  turn  rate  is  zero  (i.e.  straight  line  motion),  these  simplify  in  the  limit  to 

Vt+ 1[!]  =  Vt [1]  +  y*[4]  COS  (y*[3])  (rt+i  -  rt) ,  (36) 

Vt+ 1[2]  =  yt[2]  +  l/t[4]sin(yt[3])(Tt+i-Tt),  (37) 

y1+i[3l  =  y*[3]  +  yt[5]  (rt+i  -  rt) ,  (38) 

y*+i[4]  =  y<[4]  +wt[i],  (39) 

y*+i[5]  =  yt[5]+u*[2].  (40) 


The  above  time  recursions  are  used  to  predict  ahead  the  state  of  the  platform  at  scan  t  + 1  given 
the  state  at  scan  t.  The  straight  line  equations  are  used  when  the  estimated  turn  rate  is  low  to  avoid 
numerical  problems. 

The  above  state  models  the  position  and  movement  of  the  centre  of  the  rear  axle  of  the  car.  The 
sensor  is  located  at  a  position  y[  that  is  geometrically  related  to  the  platform  state  by  the  relation 


Kill] 

=  vd1  ]  +  ' 

\J (Z/  T-  a J 

)2  +  62cos(yt[3]  +  /?) , 

(41) 

wip] 

=  2/J2]  +  ' 

\ji  {L  +  a 

)2  +  62sin(yj3]  +  /?) , 

(42) 

0 

=  arctan  | 

'  6  ) 

.  L  T"  a  J 

5 

(43) 

where  the  dimensions  L,  a,  and  b  are  defined  as  shown  in  figure  7.  Using  the  values  for  this 
scenario,  as  shown  in  the  figure,  this  gives 

yld  1]  ~  yjl] +  3.813  cos  (y,  [3] +0.1315), 
y[[2]  «  yf[2]  +  3.813  sin (yt [3]  +0.1315). 

The  laser  measurements  give  the  relative  range  and  bearing  to  the  reflector  from  the  sensor. 
Thus  the  measurement  functions  are 


•^tlrfl]  — 

\/ (vTi1}  -  yd1})2  +  { 

xm-yddf  +  vtiA!}, 

(44) 

Ztlr[2\  = 

.  ( ®t*[2]  —  yj[2] 

arctan  - — - j— 

V*Hi]-y{[i] 

)  -yjs]  +  i/tir[2], 

(45) 
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where  vt\r  is  the  measurement  noise. 

The  inertial  measurements  are  purely  a  function  of  the  platform  states.  Assuming  that  there  is 
no  slipping  in  the  front  wheels,  these  are  related  to  the  states  by 

Zt2r[l]  =  Vt[ 4]  +  ^t2r[l],  (46) 

zt2r  [2]  =  yt[5]  +  i*ar[2].  (47) 

It  is  obvious  that  both  the  state  evolution  and  the  measurement  equations  are  nonlinear.  Under 
Generalised  EM,  this  does  not  necessarily  require  the  PMHT  to  use  a  nonlinear  filter  to  estimate  the 
states.  Generalised  EM  states  that  choosing  a  new  state  that  increases  the  auxiliary  function,  rather 
than  optimises  it,  will  lead  to  the  same  solution  on  convergence.  So,  one  might  look  to  find  a  linear 
estimator  for  the  states  that  is  guaranteed  to  increase  the  auxiliary  function,  and  simply  perform 
more  iterations.  Alternatively,  a  nonlinear  estimator  could  be  used,  such  as  a  particle  filter.  For  this 
problem,  the  benchmark  is  a  stacked  state  vector  EKF  solution,  so  it  will  be  assumed  that  the  EKF 
approximation  to  the  nonlinear  problem  is  sufficient.  Namely,  the  PMHT  will  be  implemented 
using  a  bank  of  EKFs  for  the  landmarks,  and  an  EKF  for  the  platform. 


5.3.1  EKF  Implementation 

The  EKFs  used  for  the  landmarks  are  standard  polar  measurement,  cartesian  constant  state 
filters.  So  the  state  evolution  matrix,  Fx ,  and  the  process  noise  matrix,  Gx,  are  identity.  The  process 
noise  covariance  is  very  small,  since  the  landmarks  are  non-dynamic,  Qx  =  10— 16 1  (where  I  is  the 
identity  matrix).  The  measurement  matrix  is  given  by  the  Jacobian  of  the  measurement  function 

r  AXr/r?  AY™ /r™  1 

*  [  -A Yt™/  (rD2  AX?/  (rt-)2  J  ’  ^ 

where 

Axr  =  *ni]-y*[i], 

A  Yt™  =  x™[2\  —  yf[2], _ 

r?  =  vWW  -l/t[l])2  +  (^r[2]  -1/J2])2. 


The  sensor  platform  estimator  has  been  implemented  as  a  stacked  measurement  Extended 
Kalman  Smoother.  Since  the  dynamics  are  nonlinear,  the  evolution  matrix  is  also  a  Jacobian 
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The  process  noise  matrix  is  given  by 
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(50) 


i.e.  the  process  noise  is  white  noise  acceleration  in  speed  and  steering  angle.  The  process  noise 
covariance  is  the  diagonal  matrix,  Q1/  = 

For  laser  measurements,  the  platform  estimator  uses  a  stacked  measurement  matrix,  similar  to 
(10),  but  where  the  elements  are  of  the  form  of  (48). 

For  inertia]  measurements,  the  measurement  matrix  is  given  by 
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(51) 


Both  sensors  are  assumed  to  have  diagonal  measurement  covariances  (e.g.  the  range  measure¬ 
ment  is  not  correlated  with  the  angle  measurement).  These  are  given  by 

^laser  = 

D 

inertial 

The  values  of  measurement  covariance  and  process  covariance  were  chosen  by  trial  and  error 
to  give  good  performance. 


0.1  0 
0  0.001 


0.1 
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0 

0.1 


(52) 

(53) 


5.3.2  Implementation  Issues 

There  are  a  few  other  issues  that  had  to  be  addressed  to  get  good  performance  on  the  Victoria 
Park  data.  There  are  two  main  issues  to  deal  with:  occlusion  and  loop  closing. 
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As  shown  earlier,  some  of  the  landmarks  in  the  sensor  field-of-view  may  be  completely  or 
partially  occluded  by  other  landmarks  at  closer  range.  This  causes  valid  tracks  to  be  truncated 
if  standard  target  existence  type  models  are  used  for  track  maintenance.  In  addition,  the  laser 
sensor  appears  to  be  somewhat  unreliable  even  when  there  are  no  occlusions,  especially  at  longer 
range.  These  features  make  it  difficult  to  properly  deal  with  landmark  existence.  The  approach 
taken  was  to  insert  new  tracks,  and  if  they  survived  for  longer  than  a  threshold  duration,  then  they 
were  protected  and  never  truncated.  A  simplistic  effort  to  account  for  occlusion  was  incorporated, 
but  this  was  conservative:  only  tracks  that  were  certain  to  be  occluded  were  discounted,  and  used 
primarily  to  limit  the  number  of  tracks  used  in  data  association. 

Loop  closing  becomes  an  issue  because  the  sensor  measurement  variance  is  relatively  low. 
After  the  platform  completes  a  loop,  there  will  be  some  residual  alignment  error  that  prevents 
data  association  of  the  old  landmarks.  The  error  is  actually  in  the  platform  state  estimate,  but  the 
algorithm  requires  that  the  measurement  variance  be  inflated  to  allow  for  correct  association.  An 
ad  hoc  scaling  of  the  measurement  variance  was  incorporated  at  times  when  orientation  error  is 
likely  to  be  high:  during  turns,  and  when  the  platform  closes  large  loops. 


5.3.3  Performance  without  inertial  input 

The  PMHT  algorithm  was  also  run  without  the  inertial  input  data.  This  did  not  require  re¬ 
tuning  of  the  process  or  measurement  noise  matrices,  and  was  simply  achieved  by  not  providing 
one  of  the  input  sources. 


5.3.4  Alignment 


The  GPS  reports  are  given  in  latitude  and  longitude  in  metres  relative  to  an  unknown  reference 
point.  In  order  to  compare  them  with  the  SLAM  estimate,  the  two  need  to  be  registered  and  time 
aligned.  The  initial  GPS  report  defines  the  starting  position  for  the  vehicle,  but  the  initial  heading 
needs  to  be  inferred.  The  GPS  reports  are  not  synchronous  with  the  platform  sensors,  so  the 
state  estimates  need  to  be  re-sampled  at  the  GPS  times.  This  is  done  using  the  evolution  model 
and  predicting  the  state  ahead  from  the  closest  earlier  time  point.  Once  the  trajectories  are  time 
aligned,  the  initial  heading  can  be  determined  using  a  least  squares  fit.  This  is  given  by 


Y^(AxtByt-AytB*t) 

tan  9  =  — - , 

Y^{AxtBxt+AytBf) 


(54) 


t 

where  the  GPS  report  at  time  t  is  (Af ,  ),  and  the  filter  output  is  (Bf ,  ).  This  gives  an  initial 

heading  of  approximately  35  degrees. 


5.4  Results 


The  platform  trajectory  estimate  produced  by  the  PMHT  algorithm  is  shown  in  figure  8.  The 
GPS  reports  are  shown  as  crosses,  the  PMHT  SLAM  estimate  with  all  inputs  as  a  solid  line,  and 
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Figure  8:  PMHT  estimated  trajectory 


the  PMHT  SLAM  estimate  with  only  laser  measurements  as  a  dashed  line.  Overall,  the  estimates 
are  relatively  close  to  the  GPS  reports.  Removing  the  inertial  measurements  did  not  significantly 
degrade  the  performance. 

The  main  discrepancy  between  the  GPS  and  PMHT  estimates  is  in  the  North-Eastern  part  of 
the  map.  This  part  of  the  trajectory  consists  of  several  loops  connected  to  the  main  area  via  a  single 
path.  Just  before  the  path  is  a  turn,  and  it  appeals  that  the  PMHT  algorithm  made  a  small  error 
in  estimating  the  magnitude  of  this  turn.  This  angular  error  translates  to  a  relatively  large  position 
error  at  the  farther  ends  of  the  path. 

Figure  9  compares  the  PMHT  SLAM  output  with  that  obtained  from  a  stacked  state  vector 
EKF  using  the  same  dynamic  models.  The  EKF  was  more  difficult  to  tune  and  required  somewhat 
different  process  noise  values  to  provide  a  reasonable  output.  The  two  estimates  are  very  close  for 
the  bulk  of  the  trajectory,  but  deviate  in  the  North-East  and  Western  parts  of  the  map.  As  described 
above,  both  of  these  sections  are  individual  loops  with  connecting  sections  along  turns.  Small 
angular  discrepancies  during  these  turns  lead  to  large  position  deviations.  The  difference  between 
these  two  outputs  is  due  to  data  association. 
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Figure  9:  PMHT  estimate  compared  with  EKF  estimate 
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5.4.1  Comparison  with  other  algorithms 

At  this  point  in  time,  other  authors  have  not  made  numerical  output  on  this  data  available. 
Montemerlo  provided  an  RMS  error  figure  between  the  FastSLAM  output  and  the  GPS  reports  of 
“approximately  4m”.  The  PMHT  SLAM  output  has  an  RMS  error  of  4.2m.  As  described  earlier, 
the  GPS  reports  suffer  from  relatively  large  jumps  as  occlusion  of  the  GPS  satellites  varies,  and  do 
not  provide  a  useful  truth  source.  It  is  likely  that  the  SLAM  output  is  more  accurate  than  the  GPS, 
and  the  figure  of  4m  is  mainly  due  to  errors  in  the  GPS. 

Independent  of  the  accuracy  of  the  GPS,  RMS  error  is  a  poor  measure  of  performance  in  this 
example.  As  discussed  above,  small  angular  errors  can  lead  to  large  position  deviations  for  long 
legs  of  the  path.  Thus  the  RMS  position  error  is  not  indicative  of  the  algorithm  performance. 

Along  with  the  data  set,  Neito  provided  an  image  of  the  estimated  trajectory  using  a  stacked 
state  vector  EKF,  overlaid  on  a  map  of  Victoria  Park.  This  image  has  been  used  in  annual  reports  of 
the  Australian  Centre  for  Field  Robotics,  and  represents  the  output  of  the  algorithm  in  [Guivant  & 
Nebot  2001a].  The  PMHT  output  was  manually  registered  onto  the  image  using  the  tree  position 
estimates  near  the  starting  point  of  the  vehicle.  Once  again,  small  angular  deviations  lead  to  large 
position  discrepancies  that  make  the  estimated  tree  locations  at  the  extremes  of  the  map  poor 
choices  for  registration.  Figure  10  shows  the  PMHT  output  overlaid  on  top  of  the  EKF  picture. 
The  image  provided  by  Neito  shows  the  EKF  output  as  a  solid  blue  line,  and  the  estimated  tree 
locations  as  circles  with  dots  at  the  centre.  The  GPS  reports  are  overlaid  on  the  image  based  on 
the  PMHT  registration  and  are  marked  as  crosses.  The  PMHT  output  is  marked  as  a  yellow  line. 

The  EKF  algorithm  does  a  better  job  of  estimating  the  trajectory  in  the  North-East  comer, 
which  was  identified  earlier  as  a  problem  with  the  PMHT.  However,  the  EKF  does  a  poor  job 
of  estimating  the  trajectory  along  the  Western  part  of  the  map,  compared  with  the  GPS  reports. 
There  are  a  few  differences  between  the  algorithms:  the  assumed  platform  model  and  assumed 
noise  variances,  and  data  association. 

Overall,  it  appears  that  the  PMHT  SLAM  output  is  roughly  equivalent  to  the  FastSLAM  out¬ 
put,  and  superior  to  that  of  the  EKF. 
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Figure  10:  PMHT  output  overlaid  on  EKF  output 
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6  Summary 

This  report  has  introduced  a  new  approach  for  solving  the  Simultaneous  Localisation  and 
Map  Building  problem  (SLAM).  The  algorithm  described  is  based  on  the  Probabilistic  Multi- 
Hypothesis  Tracker,  and  has  the  advantage  of  linear  complexity  in  the  number  of  landmarks  in  the 
sensor  field-of-view. 

The  computation  expense  of  PMHT  SLAM  was  demonstrated  to  be  significantly  lower  than 
current  SLAM  algorithms,  while  its  estimation  performance  is  superior  to  that  of  the  standard  EKF 
approach. 

The  algorithm  was  demonstrated  on  benchmark  data  from  Victoria  Park,  and  showed  compa¬ 
rable  performance  with  the  published  results  from  other  algorithms.  The  PMHT  performance  on 
this  data  might  be  improved  by  using  a  better  non-linear  estimator,  such  as  a  particle  filter,  or  an 
unscented  Kalman  filter. 

At  this  stage,  no  quantitative  comparison  has  been  performed  with  FastSLAM,  the  current  state 
of  the  art  algorithm.  This  was  deliberately  avoided  to  prevent  perceived  implementation  faults.  In 
the  future  it  would  be  ideal  to  pursue  a  collaborative  comparison  of  the  two  algorithms. 
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